/*
 * @Description: 多平面裁剪器，多个平面参数裁剪
 * @Author: Sang Hao
 * @Date: 2021-11-15 15:08:39
 * @LastEditTime: 2021-11-22 21:26:50
 * @LastEditors: Sang Hao
 */

#include <pcl/PointIndices.h>
#include <pcl/filters/plane_clipper3D.h>
#include <pcl/filters/extract_indices.h>

#include "glog/logging.h"

#include "lidar_slam/models/cloud_filter/multi_plane_clipper_filter.hpp"

namespace lidar_slam {
MultiPlaneClipperFilter::MultiPlaneClipperFilter(const YAML::Node& config_node) {
	/* 平面提取类的构造 */
	horizontal_plane_extract_ = std::make_shared<PlaneRansacExtract>(
		config_node["horizontal_plane_extract"]);
	vertical_plane_extract_ = std::make_shared<PlaneRansacExtract>(
		config_node["vertical_plane_extract"]);
	/* 平面容忍度参数 */
	plane_clipper_tolerance_ = config_node["plane_clipper_tolerance"].as<float>();
}

Eigen::Vector4f MultiPlaneClipperFilter::GetPlaneParamFromCoeffAndToler(pcl::ModelCoefficients& plane_model_coeff, 
		float plane_clipper_tolerance) {
	Eigen::Vector4f plane_param;
	plane_param << 	plane_model_coeff.values[0], 
					plane_model_coeff.values[1], 
					plane_model_coeff.values[2],
					plane_model_coeff.values[3];
	/* 给平面一点容忍度，相当于d参数绝对值变大或者变小（取决于tolerance符号），符号不变 */
	/* 这样的距离容忍度只有在abc参数归一化条件下才可 */
	/* 如果tolerance为正，则平面远离，保留平面，否则删除平面 */
	plane_param[3] += plane_param[3] / std::fabs(plane_param[3]) * plane_clipper_tolerance;

	return plane_param;
}
/****************************************************************/
/**********用等号做点云复制时，**********************/
// 指针和指针之间，无论是指针本身用等号，还是指针内容用等号，本质是同一块数据，浅拷贝；
// PointCloud和PointCloud也是如此。但用指针数据赋给非指针，或反过来，则使用两块内存，深拷贝。

/*************用copyPointCloud函数做点云复制时********************/
// 用指针的数据时，使用相同内存，即浅拷贝；
// 其他情况，指针给变量，或变量给变量，变量给指针，都是两块不同的区域，深拷贝。


/****************************************************************/

/**
 * @description: 得到多平面的平面参数
 */
// bool MultiPlaneClipperFilter::GetVectorModelCoeff(
// 		const CloudData::CLOUD_PTR& input_cloud_ptr) {
// 	CloudData::CLOUD_PTR temp_cloud_ptr (new CloudData::CLOUD());
// 	/* 这个变量到变量的复制点云应该随时深拷贝了 */
// 	pcl::copyPointCloud(*input_cloud_ptr, *temp_cloud_ptr);
	
// 	pcl::ModelCoefficients::Ptr model_coeff_ptr;
// 	pcl::PointIndices::Ptr point_indices_ptr;
// 	for (int i = 0; i < horizontal_plane_extract_->GetPlaneNum(); ++i) {
// 		model_coeff_ptr.reset(new pcl::ModelCoefficients());
// 		point_indices_ptr.reset(new pcl::PointIndices());

// 		LOG(INFO) << temp_cloud_ptr->points.size() << std::endl;
// 		bool is_extracted = horizontal_plane_extract_->Extract(
// 			temp_cloud_ptr, model_coeff_ptr, point_indices_ptr);
			
// 		if (!is_extracted) {
// 			continue;
// 		}

// 		vector_model_coeff_.push_back(*model_coeff_ptr);
			
// 		pcl::ExtractIndices<CloudData::POINT> temp_filter;
// 		temp_filter.setInputCloud(temp_cloud_ptr);
// 		temp_filter.setIndices(point_indices_ptr);
// 		temp_filter.setNegative(true); // true表示的是indices中的点都是不要的
// 		temp_filter.filter(*temp_cloud_ptr);
// 		std::cout << "分割平面后点云数量为" << temp_cloud_ptr->points.size() << std::endl;
// 		std::cout << "原来点云数量为" << input_cloud_ptr->points.size() << std::endl;
// 	}

// 	for (int i = 0; i < vertical_plane_extract_->GetPlaneNum(); ++i) {
// 		model_coeff_ptr.reset(new pcl::ModelCoefficients());
// 		point_indices_ptr.reset(new pcl::PointIndices());
		
// 		LOG(INFO) << temp_cloud_ptr->points.size() << std::endl;
		
// 		bool is_extracted = vertical_plane_extract_->Extract(
// 			temp_cloud_ptr, model_coeff_ptr, point_indices_ptr);
// 		if (!is_extracted) {
// 			continue;
// 		}
// 		if (std::fabs(model_coeff_ptr->values[3]) < 3) {
// 			continue;
// 		}
// 		vector_model_coeff_.push_back(*model_coeff_ptr);
// 				std::cout << "平面 " << i + 1 << "的参数" 
// 				  << model_coeff_ptr->values[0] << ", "
// 				  << model_coeff_ptr->values[1] << ", "
// 				  << model_coeff_ptr->values[2] << ", "
// 				  << model_coeff_ptr->values[3] << "已导入" << std::endl;
				  
// 		pcl::ExtractIndices<CloudData::POINT> temp_filter;
// 		temp_filter.setInputCloud(temp_cloud_ptr);
// 		temp_filter.setIndices(point_indices_ptr);
// 		temp_filter.setNegative(true); // true表示的是indices中的点都是不要的
// 		temp_filter.filter(*temp_cloud_ptr);
// 		std::cout << "分割平面后点云数量为" << temp_cloud_ptr->points.size() << std::endl;
// 		std::cout << "原来点云数量为" << input_cloud_ptr->points.size() << std::endl;
// 	}
// 	if (vector_model_coeff_.size() == 0) {
// 		LOG(WARNING) << "没有提取到平面！" << std::endl << std::endl;
// 		return false;
// 	}
// 	return true;
// }


bool MultiPlaneClipperFilter::Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, 
		CloudData::CLOUD_PTR& filter_cloud_ptr) {

	LOG(INFO) << "输入点云数量：" << input_cloud_ptr->points.size() << std::endl;
	CloudData::CLOUD_PTR temp_cloud_ptr (new CloudData::CLOUD());
	/* 这个变量到变量的复制点云应该随时深拷贝了 */
	pcl::copyPointCloud(*input_cloud_ptr, *temp_cloud_ptr);
	
	/* 做一些对象的定义 */
	pcl::ModelCoefficients::Ptr model_coeff_ptr;
	pcl::PointIndices::Ptr inlier_point_indices_ptr;
	pcl::PlaneClipper3D<CloudData::POINT>::Ptr plane_clipper_ptr;
    pcl::PointIndices::Ptr clip_point_indices_ptr;
	pcl::ExtractIndices<CloudData::POINT>::Ptr extract_filter_ptr;
	
	/************************/
	/* 首先进行水平面的平面裁剪 */
	/************************/
	for (int i = 0; i < horizontal_plane_extract_->GetPlaneNum(); ++i) {
		model_coeff_ptr.reset(new pcl::ModelCoefficients());
		inlier_point_indices_ptr.reset(new pcl::PointIndices());

		/* 利用提取bool值提高鲁棒性 */
		bool is_extracted = horizontal_plane_extract_->Extract(
			temp_cloud_ptr, model_coeff_ptr, inlier_point_indices_ptr);
		if (!is_extracted) {
			continue;
		}

		Eigen::Vector4f plane_param = 
			GetPlaneParamFromCoeffAndToler(*model_coeff_ptr, plane_clipper_tolerance_);

		/* 对平面裁剪器和点云索引进行reset */
		plane_clipper_ptr.reset(new pcl::PlaneClipper3D<CloudData::POINT>(plane_param));
		clip_point_indices_ptr.reset(new pcl::PointIndices());
		plane_clipper_ptr->clipPointCloud3D(*temp_cloud_ptr, clip_point_indices_ptr->indices);

		/* 定义提取滤波对象， */
		extract_filter_ptr.reset(new pcl::ExtractIndices<CloudData::POINT>());
		extract_filter_ptr->setInputCloud(temp_cloud_ptr);
		extract_filter_ptr->setIndices(clip_point_indices_ptr);
		extract_filter_ptr->setNegative(false);
		extract_filter_ptr->filter(*temp_cloud_ptr);

		/* TODO:用不用把前边的指针都回收啊 */
		inlier_point_indices_ptr->indices.clear();
		clip_point_indices_ptr->indices.clear();
		

		LOG(INFO) << "水平面 " << i + 1 << "的参数" 
			<< model_coeff_ptr->values[0] << ", "
			<< model_coeff_ptr->values[1] << ", "
			<< model_coeff_ptr->values[2] << ", "
			<< model_coeff_ptr->values[3] << "已导入" << std::endl;
				  
		LOG(INFO) 	<< "水平面" << i + 1 << "裁剪后点云数量为：" 
					<< temp_cloud_ptr->points.size() << std::endl;
	}

	/************************/
	/****再对竖直面裁剪********/
	/************************/
	for (int i = 0; i < vertical_plane_extract_->GetPlaneNum(); ++i) {
		model_coeff_ptr.reset(new pcl::ModelCoefficients());
		inlier_point_indices_ptr.reset(new pcl::PointIndices());
		
		bool is_extracted = vertical_plane_extract_->Extract(
			temp_cloud_ptr, model_coeff_ptr, inlier_point_indices_ptr);
		if (!is_extracted) {
			continue;
		}
		/* 提取的竖直面不能离原点太近 */
		if (std::fabs(model_coeff_ptr->values[3]) < 1.5) {
			continue;
		}

		Eigen::Vector4f plane_param = 
			GetPlaneParamFromCoeffAndToler(*model_coeff_ptr, plane_clipper_tolerance_);

		/* 对平面裁剪器和点云索引进行reset */
		plane_clipper_ptr.reset(new pcl::PlaneClipper3D<CloudData::POINT>(plane_param));
		clip_point_indices_ptr.reset(new pcl::PointIndices());
		/* 完成平面裁剪 */
		plane_clipper_ptr->clipPointCloud3D(*temp_cloud_ptr, clip_point_indices_ptr->indices);
				  
		/* 对平面裁剪后的点云进行滤波 */
		extract_filter_ptr.reset(new pcl::ExtractIndices<CloudData::POINT>());

		extract_filter_ptr->setInputCloud(temp_cloud_ptr);
		extract_filter_ptr->setIndices(clip_point_indices_ptr);
		extract_filter_ptr->setNegative(true); // true表示的是indices中的点都是不要的
		extract_filter_ptr->filter(*temp_cloud_ptr);

		LOG(INFO) << "竖直平面 " << i + 1 << "的参数" 
			<< model_coeff_ptr->values[0] << ", "
			<< model_coeff_ptr->values[1] << ", "
			<< model_coeff_ptr->values[2] << ", "
			<< model_coeff_ptr->values[3] << "已导入" << std::endl;
		
		LOG(INFO) 	<< "竖直面" << i + 1 << "裁剪后点云数量为：" 
					<< temp_cloud_ptr->points.size() << std::endl;
	}

	/* 将结果保留到filter_cloud_ptr中 */
	pcl::copyPointCloud(*temp_cloud_ptr, *filter_cloud_ptr);
	return true;
}
}
